#include "Camera.h"
#include "../resource.h"
#include "../base/Base.h"
#include "../base/RNG.h"
#include "../3rdparty/BRDFReader.h"
#include "../MainFrm.h"
#include "../ReflectometryDoc.h"
#include "../sky/ClearSky.h"

#include <fstream>
using namespace std;

// for debugging
#include <io.h>
#include <fcntl.h>
//	end


void CCamera::InitConsole()
{
	int nRet= 0;  
	FILE* fp;  
	AllocConsole();  
	nRet= _open_osfhandle((long)GetStdHandle(STD_OUTPUT_HANDLE), _O_TEXT);  
	fp = _fdopen(nRet, "w");  
	*stdout = *fp;  
	setvbuf(stdout, NULL, _IONBF, 0);  
}

CCamera::CCamera()
{
	m_intrinsicMatrix = cvCreateMat(3, 3, CV_32FC1);
	m_distortionCoeffs = cvCreateMat(4, 1, CV_32FC1);

	char buffer[256];
	CString saveFilePath;
	saveFilePath.LoadStringW(IDS_STRING_CAMERA_INTRINSIC);
	Str2Char(saveFilePath, buffer);
	m_intrinsicMatrix = (CvMat *)cvLoad(buffer);
	
	saveFilePath.LoadStringW(IDS_STRING_CAMERA_DISTORTION);
	Str2Char(saveFilePath, buffer);
	m_distortionCoeffs = (CvMat *)cvLoad(buffer);

	m_brdfReader.LoadBRDF();
	m_brdfPhotos.clear();
	InitConsole();
}

CCamera::~CCamera()
{
	if (m_intrinsicMatrix)
	{
		cvReleaseMat(&m_intrinsicMatrix);
		m_intrinsicMatrix = NULL;
	}
	if (m_distortionCoeffs)
	{
		cvReleaseMat(&m_distortionCoeffs);
		m_distortionCoeffs = NULL;
	}
}

void CCamera::CalcIntrinsicParas(int boardWidth, int boardHeight, vector<CString> imagePaths)
{
	const int board_dt = 5;
	boardWidth--;
	boardHeight--;

//	CvCapture* capture;

	int board_n  = boardWidth * boardHeight;
	CvSize board_sz = cvSize(boardWidth, boardHeight);
	int n_boards = imagePaths.size();

	//	allocate storage
	CvMat* image_points = cvCreateMat(n_boards * board_n, 2, CV_32FC1);
	CvMat* object_points = cvCreateMat(n_boards * board_n, 3, CV_32FC1);
	CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);

	if (m_intrinsicMatrix)
	{
		cvReleaseMat(&m_intrinsicMatrix);
		m_intrinsicMatrix = NULL;
	}
	if (m_distortionCoeffs)
	{
		cvReleaseMat(&m_distortionCoeffs);
		m_distortionCoeffs = NULL;
	}
	m_intrinsicMatrix = cvCreateMat(3, 3, CV_32FC1);
	m_distortionCoeffs = cvCreateMat(4, 1, CV_32FC1);

	IplImage* image = 0;
	IplImage* gray_image = 0;
	CvPoint2D32f* corners = new CvPoint2D32f[board_n];
	int corner_count;
	int successes = 0;
	int step;


	char buffer[256];

	for (int frame = 0; frame < n_boards; frame++) 
	{
		if(image)
		{
			cvReleaseImage(&image);
			image = 0;
		}
		Str2Char(imagePaths[frame], buffer);
		image = cvLoadImage(buffer);
		if(gray_image == 0 && image)
			gray_image = cvCreateImage(cvGetSize(image), 8, 1);

		if(!image)
			printf("null image\n");

		int found = cvFindChessboardCorners(
			image,
			board_sz,
			corners,
			&corner_count, 
			CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
			);

		cvCvtColor(image, gray_image, CV_BGR2GRAY);
		cvFindCornerSubPix(gray_image, corners, corner_count, 
			cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

		cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
		cvShowImage( buffer, image );
		if(corner_count == board_n) 
		{
			step = successes * board_n;
			for(int i = step, j = 0; j < board_n; ++i, ++j) 
			{
				CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
				CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;
				CV_MAT_ELEM(*object_points, float, i, 0) = (float)(j / boardWidth);
				CV_MAT_ELEM(*object_points, float, i, 1) = (float)(j % boardWidth);
				CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;
			}
			CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;		
			successes++;
		}
	}

	//	allocate matrices according to how many images we found chessboards on
	CvMat* object_points2 = cvCreateMat(successes * board_n, 3, CV_32FC1);
	CvMat* image_points2 = cvCreateMat(successes * board_n, 2, CV_32FC1);
	CvMat* point_counts2 = cvCreateMat(successes, 1, CV_32SC1);

	//	transfer the points into the correct size matrices
	for(int i = 0; i < successes * board_n; ++i)
	{
		CV_MAT_ELEM(*image_points2, float, i, 0) = CV_MAT_ELEM(*image_points, float, i, 0);
		CV_MAT_ELEM(*image_points2, float, i, 1) = CV_MAT_ELEM(*image_points, float, i, 1);
		CV_MAT_ELEM(*object_points2,float, i, 0) = CV_MAT_ELEM(*object_points, float, i, 0);
		CV_MAT_ELEM(*object_points2,float, i, 1) = CV_MAT_ELEM(*object_points, float, i, 1);
		CV_MAT_ELEM(*object_points2,float, i, 2) = CV_MAT_ELEM(*object_points, float, i, 2);

	} 
	for(int i = 0; i < successes; ++i)
	{
		CV_MAT_ELEM(*point_counts2, int, i, 0) = CV_MAT_ELEM(*point_counts, int, i, 0);
	}
	cvReleaseMat(&object_points);
	cvReleaseMat(&image_points);
	cvReleaseMat(&point_counts);

	CV_MAT_ELEM( *m_intrinsicMatrix, float, 0, 0) = 1.0f;
	CV_MAT_ELEM( *m_intrinsicMatrix, float, 1, 1) = 1.0f;
	cvCalibrateCamera2(
		object_points2,
		image_points2,
		point_counts2,
		cvGetSize(image),
		m_intrinsicMatrix,
		m_distortionCoeffs,
		NULL,
		NULL,
		0
		);
	// save our work

	CString saveFilePath;
	saveFilePath.LoadStringW(IDS_STRING_CAMERA_INTRINSIC);
	Str2Char(saveFilePath, buffer);
	cvSave(buffer, m_intrinsicMatrix);
	saveFilePath.LoadStringW(IDS_STRING_CAMERA_DISTORTION);
	Str2Char(saveFilePath, buffer);
	cvSave(buffer, m_distortionCoeffs);
}

CvMat* CCamera::GetIntrinsicMatrix()
{
	return m_intrinsicMatrix;
}

CvMat* CCamera::GetDistortionCoeffs()
{
	return m_distortionCoeffs;
}

void CCamera::CameraPoint3D2Pixel(CPoint3D point, CPixel &pixel)
{
	CvMat* rotationVector = cvCreateMat(1, 3, CV_32FC1);
	CvMat* translationVector = cvCreateMat(1, 3, CV_32FC1);
	CvMat* cvPoint = cvCreateMat(1, 3, CV_32FC1);
	CvMat* cvPixel = cvCreateMat(1, 2, CV_32FC1);

	CV_MAT_ELEM(*cvPoint, float, 0, 0) = point.m_x;
	CV_MAT_ELEM(*cvPoint, float, 0, 1) = point.m_y;
	CV_MAT_ELEM(*cvPoint, float, 0, 2) = point.m_z;
	
	for (int i = 0; i < 3; i++)
	{
		CV_MAT_ELEM(*rotationVector, float, 0, i) = 0.0f;
		CV_MAT_ELEM(*translationVector, float, 0, i) = 0.0f;
	}
	cvProjectPoints2(
		cvPoint,
		rotationVector,
		translationVector,
		m_intrinsicMatrix,
		m_distortionCoeffs,
		cvPixel,
		NULL, 
		NULL, 
		NULL, 
		NULL, 
		NULL, 
		0
	);
	pixel.m_x = (int)CV_MAT_ELEM(*cvPixel, float, 0, 0);
	pixel.m_y = (int)CV_MAT_ELEM(*cvPixel, float, 0, 1);

	cvReleaseMat(&rotationVector);
	cvReleaseMat(&translationVector);
	cvReleaseMat(&cvPoint);
	cvReleaseMat(&cvPixel);
}

void CCamera::Pixel2CameraPoint3D(CPixel pixel, CPoint3D &point)
{
	//	omit distortions
	float x = (float)pixel.m_x;
	float y = (float)pixel.m_y;
	float cx = CV_MAT_ELEM(*m_intrinsicMatrix, float, 0, 2);
	float cy = CV_MAT_ELEM(*m_intrinsicMatrix, float, 1, 2);
	float fx = CV_MAT_ELEM(*m_intrinsicMatrix, float, 0, 0);
	float fy = CV_MAT_ELEM(*m_intrinsicMatrix, float, 1, 1);

	point.m_z = fx;
	point.m_x = (x - cx);
	point.m_y = (y - cy) / fy * fx;
}

void CCamera::AddNewBRDFPhoto(CBRDFPhoto photo)
{
	m_brdfPhotos.push_back(photo);
}

void CCamera::CalcBRDFPhotoParas()
{
	int n = m_brdfPhotos.size();
	for (int i = 0; i < n; i++)
		m_brdfPhotos[i].ComputeExtrinsicParas();
}

void CCamera::CalcBRDFPhotoParas(int index)
{
	m_brdfPhotos[index].ComputeExtrinsicParas();
}

void CCamera::InitSampleFromBRDFPhoto(int index)
{
	m_brdfPhotos[index].InitPoints();
}

void CCamera::SampleFromBRDFPhoto(int index, float opx, float opy, float opz, float ipx, float ipy)
{
	m_brdfPhotos[index].AddPoints(opx, opy, opz, ipx, ipy);
}

CBRDFPhoto CCamera::GetBRDFPhoto(int index)
{
	return m_brdfPhotos[index];
}

void CCamera::CalcBRDF(int sphereSamples, int xRes, int yRes, float xSize, float ySize, int neighbour)
{
	RNG rng;
	//	disk uniform sampling
	CPoint3D* pSphereSamples = new CPoint3D[sphereSamples];
	float r, theta;
	for (int i = 0; i < sphereSamples; i++)
	{
		r = sqrt(rng());
		theta = 2.0f * M_PI * rng();
		pSphereSamples[i].m_x = r * cosf(theta);
		pSphereSamples[i].m_y = r * sinf(theta);
		pSphereSamples[i].m_z = sqrt(1 
			- pSphereSamples[i].m_x * pSphereSamples[i].m_x 
			- pSphereSamples[i].m_y * pSphereSamples[i].m_y);
	}

	//	compute brdf for each solution
	int length = m_brdfPhotos.size();
	CColor* pColor = new CColor[length];


// 	double red, green, blue;
// 	brdfReader.CalcBRDF(0, M_PI / 4, 0, M_PI / 4, M_PI, red, green, blue);
	
	CColor* pMaterialColor[MATERIAL_COUNT];
	for (int i = 0; i < MATERIAL_COUNT; i++)
	{
		pMaterialColor[i] = new CColor[length];
		for (int j = 0; j < length; j++)
		{
			pMaterialColor[i][j].m_r = 0.0f;
			pMaterialColor[i][j].m_g = 0.0f;
			pMaterialColor[i][j].m_b = 0.0f;
		}
	}

// 	CvMat *A[3];
// 	CvMat *x[3];
// 	CvMat *y[3];
// 	for (int i = 0; i < 3; i++)
// 	{
// 		A[i] = cvCreateMat(length, neighbour, CV_32FC1);
// 		x[i] = cvCreateMat(neighbour, 1, CV_32FC1);
// 		y[i] = cvCreateMat(length, 1, CV_32FC1);
// 	}
	CPoint3D gloPoint;
	gloPoint.m_z = 0.0f;
	CColor *flags = new CColor[neighbour];

// 	ofstream fout[3];
// 	fout[0].open("./data/outputr.brdf");
// 	fout[1].open("./data/outputg.brdf");
// 	fout[2].open("./data/outputb.brdf");
	float deltax = 1.0f / xRes * xSize;
	float deltay = 1.0f / yRes * ySize;
	gloPoint.m_x = 0.0f;
	CColor *pWeight;
	pWeight = new CColor[neighbour];
	ofstream fout("./data/brdf.data");

	fout << xSize << ' ' << ySize << endl;
	fout << xRes << ' ' << yRes << endl;
	fout << sphereSamples << ' ' << neighbour << endl;	
	CColor*** pWriteToFileColor = new CColor**[length];
	for (int i = 0; i < length; i++)
	{
		pWriteToFileColor[i] = new CColor*[xRes];
		for (int j = 0; j < xRes; j++)
			pWriteToFileColor[i][j] = new CColor[yRes];
	}

	CColor pNearestColor[20];
	for (int i = 0; i < xRes; i++, gloPoint.m_x += deltax)
	{
		gloPoint.m_y = 0.0f;
		cout << i << endl;
		for (int j = 0; j < yRes; j++, gloPoint.m_y += deltay)
		{
//			fout[0] << i << ' ' << j << endl;
//			fout[1] << i << ' ' << j << endl;
//			fout[2] << i << ' ' << j << endl;
			fout << i << ' ' << j << endl;
			GetPhotoColor(pColor, gloPoint.m_x, gloPoint.m_y,gloPoint.m_z);
//			GetPhotoColor(y[0], y[1], y[2], gloPoint.m_x, gloPoint.m_y, gloPoint.m_z);
			//	compute material colors for (opx, opy, opz)
			for (int k = 0; k < MATERIAL_COUNT; k++)
			{
				//	compute pMaterialColor[k]
				GetMaterialColor(pMaterialColor[k], gloPoint, 
					pSphereSamples, sphereSamples, &m_brdfReader, k);
			}
			fout << Solve(pColor, pMaterialColor, length, neighbour, flags, pWeight, pNearestColor) << endl;
			
			for (int len = 0; len < length; len++)
			{
				pWriteToFileColor[len][i][j] = pNearestColor[len];
			}

			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < neighbour; j++)
				{
					fout << flags[j][i] << ' ' << pWeight[j][i] << ' ';
				}
				fout << endl;
			}
// 			for (int i = 0; i < 3; i++)
// 			{
//
//				FindNeighbour(y[i], pMaterialColor, neighbour, flags, i);
//				for (int j = 0; j < neighbour; j++)
//					fout[i] << flags[j] << ' ';
//				fout[i] << endl;
//				FillA(A[i], pMaterialColor, flags, i);
//				fout[i] << SolveX(A[i], x[i], y[i]) << endl;
// 				for (int j = 0; j < neighbour; j++)
// 				{
// 					temp = CV_MAT_ELEM(*x[i], float, j, 0);
// 					fout[i] << temp << ' ';
// 				}
//				fout[i] << endl;
//			}
		}
	}

	char filename[256];
	filename[1] = '\0';
	
	for (int i = 0; i < length; i++)
	{
		filename[0] = i + '0';
		fout.open(filename);
		for (int j = 0; j < xRes; j++)
		{
			for (int k = 0; k < yRes; k++)
			{
				fout << pWriteToFileColor[i][j][k][0] << ' '
				<< pWriteToFileColor[i][j][k][1] << ' '
				<< pWriteToFileColor[i][j][k][2] << endl;
			}
		}
		fout.close();
	}
// 	fout[0].close();
// 	fout[1].close();
// 	fout[2].close();
	for (int i = 0; i < MATERIAL_COUNT; i++)
		delete []pMaterialColor[i];

	for (int i = 0; i < length; i++)
	{
		for (int j = 0; j < xRes; j++)
			delete []pWriteToFileColor[i][j];
		delete []pWriteToFileColor[i];
	}
	delete []pWriteToFileColor;

	delete []pWeight;
	delete []pColor;
	delete []pSphereSamples;
	delete []flags;
	fout.close();
// 	for (int i = 0; i < 3; i++)
// 	{
// 		cvReleaseMat(&A[i]);
// 		cvReleaseMat(&x[i]);
// 		cvReleaseMat(&y[i]);
// 	}
}

CBRDFPhoto CCamera::EvalBRDF(int brdfPhotoIndex)
{
// 	ifstream fin("./data/brdf.data");
// 	int xRes, yRes;
// 	float xSize, ySize;
// 	int sphereSamples, neighbour;
// 	fin >> xSize >> ySize;
// 	fin >> xRes >> yRes;
// 	fin >> sphereSamples >> neighbour;
// 
// 	CBRDFPhoto photo(m_brdfPhotos[brdfPhotoIndex]);
// 	CPoint3D gloPoint;
// 	CPixel pixel;
// 	int scale = 8;
// 
// 	RNG rng;
// 	//	disk uniform sampling
// 	CPoint3D* pSphereSamples = new CPoint3D[sphereSamples];
// 	float r, theta;
// 	for (int i = 0; i < sphereSamples; i++)
// 	{
// 		r = sqrt(rng());
// 		theta = 2.0f * M_PI * rng();
// 		pSphereSamples[i].m_x = r * cosf(theta);
// 		pSphereSamples[i].m_y = r * sinf(theta);
// 		pSphereSamples[i].m_z = sqrt(1 
// 			- pSphereSamples[i].m_x * pSphereSamples[i].m_x 
// 			- pSphereSamples[i].m_y * pSphereSamples[i].m_y);
// 	}
// 
// 	int length = m_brdfPhotos.size();
// 
// 	CColor pMaterialColor;
// 
// 	gloPoint.m_z = 0.0f;
// 	CColor *flags = new CColor[neighbour];
// 
// 	float deltax = 1.0f / xRes * xSize;
// 	float deltay = 1.0f / yRes * ySize;
// 	gloPoint.m_x = 0.0f;
// 	CColor *pWeight;
// 	pWeight = new CColor[neighbour];
// 
// 	int num;
// 	float fnum;
// 	ofstream fout("./data/err.data");
// 
// 	CColor color;
// 	CColor** pColor = new CColor*[xRes];
// 	for (int i = 0; i < xRes; i++)
// 		pColor[i] = new CColor[yRes];
// 	for (int i = 0; i < xRes; i++, gloPoint.m_x += deltax)
// 	{
// 		gloPoint.m_y = 0.0f;
// 		cout << i << endl;
// 		for (int j = 0; j < yRes; j++, gloPoint.m_y += deltay)
// 		{
// 			fin >> num >> num;
// 			fin >> fnum;
// 			fout << i << ' ' << j << endl;
// 			for (int clIndex1 = 0; clIndex1 < 3; clIndex1++)
// 				for (int nlIndex = 0; nlIndex < neighbour; nlIndex++)
// 					fin >> flags[nlIndex][clIndex1] >> pWeight[nlIndex][clIndex1];
// 			photo.GlobalPoint3D2Color(gloPoint, color);			
// 			for (int clIndex = 0; clIndex < 3; clIndex++)
// 			{
// 				pColor[i][j][clIndex] = 0.0f;
// 				for (int k = 0; k < neighbour; k++)
// 				{
// 					//	compute pMaterialColor[k]
// 					GetMaterialColor(pMaterialColor, gloPoint, 
// 						pSphereSamples, sphereSamples, &m_brdfReader, flags[k][clIndex], brdfPhotoIndex);
// 					pColor[i][j][clIndex] += pWeight[k][clIndex] * pMaterialColor[clIndex];
// 				}
// 			}
// 			fnum = 0.0f;
// 			for (int k = 0; k < 3; k++)
// 			{
// 				fnum += (color[k] - pColor[i][j][k]) * (color[k] - pColor[i][j][k]);
// 			}
// 			fout << fnum << endl;
// 			//Solve(pColor, pMaterialColor, length, neighbour, flags, pWeight);
// 		}
// 	}

	ifstream fin("./data/brdf.data");
	int xRes, yRes;
	float xSize, ySize;
	int sphereSamples, neighbour;
	fin >> xSize >> ySize;
	fin >> xRes >> yRes;
	fin >> sphereSamples >> neighbour;
	fin.close();

	CColor**pColor = new CColor*[xRes];
	for (int i = 0; i < xRes; i++)
		pColor[i] = new CColor[yRes];

	char filename[256];
	filename[0] = brdfPhotoIndex + '0';
	filename[1] = '\0';
	fin.open(filename);
	for (int i = 0; i < xRes; i++)
	{
		for (int j = 0; j < yRes; j++)
		{
			fin >> pColor[i][j][0] >> pColor[i][j][1] >> pColor[i][j][2];
		}
	}

	

	int scale = 8;
	float xStep = xSize / xRes;
	float yStep = ySize / yRes;
	float xScaleStep = xStep / scale;
	float yScaleStep = yStep / scale;
	
	float paras[2][2];
	float u, v;
	CColor color;
	CPoint3D gloPoint;
	CBRDFPhoto photo(m_brdfPhotos[brdfPhotoIndex]);
	CPixel pixel;
	for (int i = 0; i < xRes - 1; i++)
	{
		for (int j = 0; j < yRes - 1; j++)
		{
			for (int si = 0; si < scale; si++)
			{
				for (int sj = 0; sj < scale; sj++)
				{
					u = si * 1.0f / scale;
					v = sj * 1.0f / scale;
					paras[0][0] = (1 - u) * (1 - v);
					paras[0][1] = u * (1 - v);
					paras[1][0] = (1 - u) * v;
					paras[1][1] = u * v;
					for (int clIndex = 0; clIndex < 3; clIndex++)
					{
						color[clIndex] = pColor[i][j][clIndex] * paras[0][0]
							+ pColor[i][j + 1][clIndex] * paras[0][1]
							+ pColor[i + 1][j][clIndex] * paras[1][0]
							+ pColor[i + 1][j + 1][clIndex] * paras[1][1];
					}
					
					gloPoint.SetPoint(i * xStep + si * xScaleStep, j * yStep + sj * yScaleStep, 0.0f);
					photo.GlobalPoint3D2Pixel(gloPoint, pixel);
					
					photo.SetPixelColor(pixel.m_x, pixel.m_y, color[0], color[1], color[2]);
				}
			}
		}
	}
	photo.SavePhoto("./data/rec.bmp");
	for (int i = 0; i < xRes; i++)
		delete []pColor[i];
	delete []pColor;

	return photo;
	
	fin.close();
}

void CCamera::FindNearestMaterial(CColor pColor[], CColor* pMaterialColor[], int nPhotos, int colorIndex, int &index, float &weight)
{
	float distance;
	float pColorLength, pMaterialColorLength, innerProduct;
	pColorLength = 0.0f;
	for (int i = 0; i < nPhotos; i++)
	{
		pColorLength += pColor[i][colorIndex] * pColor[i][colorIndex];
	}
	float minDistance = pColorLength;
	index = 0;
	weight = 0.0f;
	for (int i = 0; i < MATERIAL_COUNT; i++)
	{
		pMaterialColorLength = innerProduct = 0.0f;
		for (int j = 0; j < nPhotos; j++)
		{
			pMaterialColorLength += pMaterialColor[i][j][colorIndex] * pMaterialColor[i][j][colorIndex];
			innerProduct += pMaterialColor[i][j][colorIndex] * pColor[j][colorIndex];
		}
		distance = pColorLength - (innerProduct * innerProduct) / pMaterialColorLength;
		if (distance < minDistance && innerProduct > 0)
		{
			weight = innerProduct / pMaterialColorLength;
			index = i;
			minDistance = distance;
		}
	}
	//	update pColor
	for (int i = 0; i < nPhotos; i++)
	{
		pColor[i][colorIndex] -= weight * pMaterialColor[index][i][colorIndex];
	}
}

float CCamera::Solve(CColor pColor[], CColor* pMaterialColor[], 
	int nPhotos, int nNeighbours, CColor flag[], CColor weight[],
	CColor pNearestColor[])
{
	//	now that we have pColor(nPhoto * 1)
	//	pMaterialColor(nPhoto * 100)
	//	nNeighbours
	//	what we need to do is to:
	//	step 1: find the nNeighbour-nearest between pMaterialColor and pColor
	//	step 2: compute the non negative linear combination of the neighbors
	//	step 3: write the index of the material int of flag[]
	//			and the weight into weight[]

	int colorIndex = 0;
	int index;
	float w;
	for (colorIndex = 0; colorIndex < 3; colorIndex++)
	{
		for (int i = 0; i < nPhotos; i++)
			pNearestColor[i][colorIndex] = 0.0f;
		for (int i = 0; i < nNeighbours; i++)
		{
			FindNearestMaterial(pColor, pMaterialColor, nPhotos,colorIndex, index, w);
			flag[i][colorIndex] = index;
			weight[i][colorIndex] = w;
			for (int j = 0; j < nPhotos; j++)
				pNearestColor[j][colorIndex] += w * pMaterialColor[index][j][colorIndex];
		}
	}
	float sum = 0.0f;
	for (int i = 0; i < nPhotos; i++)
		for (int j = 0; j < 3; j++)
		{
			sum += pColor[i][j] * pColor[i][j];
		}
	return sum;
}

void CCamera::GetPhotoColor(CColor pColor[], float x, float y, float z)
{
	CPoint3D point;
	point.SetPoint(x, y, z);
	int n = m_brdfPhotos.size();
	for (int i = 0; i < n; i++)
	{
		m_brdfPhotos[i].GlobalPoint3D2Color(point, pColor[i]);
	}
} 

void CCamera::GetPhotoColor(CvMat* ry, CvMat* gy, CvMat* by, float x, float y, float z)
{
	CColor color;
	CPoint3D point;
	point.SetPoint(x, y, z);
	int n = m_brdfPhotos.size();
	for (int i = 0; i < n; i++)
	{
		m_brdfPhotos[i].GlobalPoint3D2Color(point, color);
		CV_MAT_ELEM(*ry, float, i, 0) = color.m_r;
		CV_MAT_ELEM(*gy, float, i, 0) = color.m_g;
		CV_MAT_ELEM(*by, float, i, 0) = color.m_b;
	}
}

void CCamera::GetMaterialColor(CColor pColor[], 
	CPoint3D gloPoint,
	CPoint3D samples[],
	int nSamples,
	CBRDFReader* pReader, int index)
{
	int n = m_brdfPhotos.size();
	double termR, termG, termB;
	int LightR, LightG, LightB;
	float factor = M_PI / nSamples;
	CMainFrame* pMainFrame = (CMainFrame *)AfxGetMainWnd();
	CReflectometryDoc *pDoc = (CReflectometryDoc *)pMainFrame->GetActiveDocument();
	CClearSky *pSky = pDoc->GetClearSky();

	CPoint3D outLight, origin;
	origin.SetPoint(0.0f, 0.0f, 0.0f);
	for (int i = 0; i < n; i++)
	{
		//	compute pColor[i]
		pColor[i].m_r = pColor[i].m_g = pColor[i].m_b = 0.0f;
		//	sample in the i-th photos
		m_brdfPhotos[i].CameraPoint3D2GlobalPoint3D(origin, outLight);
		outLight.m_x -= gloPoint.m_x;
		outLight.m_y -= gloPoint.m_y;
		outLight.m_z -= gloPoint.m_z;
		for (int j = 0; j < nSamples; j++)
		{
			pSky->Glo3DPoint2SkyModelColor(samples[j].m_x, samples[j].m_y, samples[j].m_z, LightR, LightG, LightB);
			pReader->CalcBRDF(index, samples[j], outLight, termR, termG, termB);
			pColor[i].m_r += LightR * termR;
			pColor[i].m_g += LightG * termG;
			pColor[i].m_b += LightB * termB;
		}
		pColor[i].m_r *= factor;
		pColor[i].m_g *= factor;
		pColor[i].m_b *= factor;
	}
}

void CCamera::GetMaterialColor(CColor& color, 
	CPoint3D gloPoint,
	CPoint3D samples[],
	int nSamples,
	CBRDFReader* pReader, int matIndex, int brdfPhotoIndex)
{
	int n = m_brdfPhotos.size();
	double termR, termG, termB;
	int LightR, LightG, LightB;
	float factor = M_PI / nSamples;
	CMainFrame* pMainFrame = (CMainFrame *)AfxGetMainWnd();
	CReflectometryDoc *pDoc = (CReflectometryDoc *)pMainFrame->GetActiveDocument();
	CClearSky *pSky = pDoc->GetClearSky();

	CPoint3D outLight, origin;
	origin.SetPoint(0.0f, 0.0f, 0.0f);
	
	color.m_r = color.m_g = color.m_b = 0.0f;
	//	sample in the i-th photos
	m_brdfPhotos[brdfPhotoIndex].CameraPoint3D2GlobalPoint3D(origin, outLight);
	outLight.m_x -= gloPoint.m_x;
	outLight.m_y -= gloPoint.m_y;
	outLight.m_z -= gloPoint.m_z;
	for (int j = 0; j < nSamples; j++)
	{
		pSky->Glo3DPoint2SkyModelColor(samples[j].m_x, samples[j].m_y, samples[j].m_z, LightR, LightG, LightB);
		pReader->CalcBRDF(matIndex, samples[j], outLight, termR, termG, termB);
		color.m_r += LightR * termR;
		color.m_g += LightG * termG;
		color.m_b += LightB * termB;
	}
	color.m_r *= factor;
	color.m_g *= factor;
	color.m_b *= factor;
}

void CCamera::QuickSort(CPoint3D pArray[], int start, int end)
{
	if (start >= end)
		return;

	//	compute mid
	int i = start;
	CPoint3D temp;
	for (int j = start + 1; j <= end; j++)
	{
		if (pArray[j].m_x < pArray[start].m_x)
		{
			i++;
			//	swap i and j
			if (i != j)
			{
				temp = pArray[i];
				pArray[i] = pArray[j];
				pArray[j] = temp;
			}
		}
	}

	//	swap start and i
	temp = pArray[start];
	pArray[start] = pArray[i];
	pArray[i] = temp;

	QuickSort(pArray, start, i - 1);
	QuickSort(pArray, i + 1, end);
}

void CCamera::FindNeighbour(CvMat* y, CColor* pColor[], int neighbour, int flag[], int color)
{
	ofstream fout("./data/test");
	CPoint3D distance[MATERIAL_COUNT];
	CvSize size = cvGetSize(y);
	float* pa = new float[size.height];
	float la = 0.0f;
	fout << "y" << endl;
	for (int i = 0; i < size.height; i++)
	{
		pa[i] = CV_MAT_ELEM(*y, float, i, 0);
		la += pa[i] * pa[i];
		fout << pa[i] << ' ';
	}
	fout << endl;
	float lb = 0.0f;
	float innerProduct;
	fout << "materials" << endl;
	for (int i = 0; i < MATERIAL_COUNT; i++)
	{
		distance[i].m_y = i;
		lb = 0.0f;
		innerProduct = 0.0f;
		for (int j = 0; j < size.height; j++)
		{
			fout << pColor[i][j][color] << ' ';
			lb += pColor[i][j][color] * pColor[i][j][color];
			innerProduct += pa[j] * pColor[i][j][color];
		}
		distance[i].m_x = (la * lb - innerProduct * innerProduct) / (lb);
		fout << endl;
	}

	//	qsort distance
	QuickSort(distance, 0, MATERIAL_COUNT - 1);


// 	float ftemp;
// 	int itemp;
// 	for (int i = 0; i < MATERIAL_COUNT - 1; i++)
// 	{
// 		for (int j = MATERIAL_COUNT - 2; j >= i; j--)
// 		{
// 			if (distance[j] > distance[j + 1])
// 			{
// 				ftemp = distance[j];
// 				distance[j] = distance[j + 1];
// 				distance[j + 1] = ftemp;
// 				itemp = index[j];
// 				index[j] = index[j + 1];
// 				index[j + 1] = itemp;
// 			}
// 		}
// 	}


	for (int i = 0; i < neighbour; i++)
	{
		flag[i] = (int)(distance[i].m_y);
	}
	delete []pa;
	fout.close();
}

void CCamera::FillA(CvMat* A, CColor* pColor[], int flag[], int colorIndex)
{
	CvSize size = cvGetSize(A);
	int row = size.height;
	int col = size.width;
	int nCol = 0;
	for (int i = 0; i < col; i++)
	{
		for (int j = 0; j < row; j++)
			CV_MAT_ELEM(*A, float, j, i) = pColor[flag[i]][j][colorIndex];
	}
}

float CCamera::SolveX(CvMat* A, CvMat* x, CvMat* y)
{
	//	find the least-square solution to Ax = y => x = (A'A)^(-1)A'y
	CvSize size = cvGetSize(A);
	int row = size.height;
	int col = size.width;
	CvMat* AtA = cvCreateMat(col, col, CV_32FC1);
	cvGEMM(A, A, 1, NULL, 0, AtA, CV_GEMM_A_T);
	CvMat* invAtA = cvCreateMat(col, col, CV_32FC1);
	cvInvert(AtA, invAtA);
	CvMat* Aty = cvCreateMat(col, 1, CV_32FC1);
	cvGEMM(A, y, 1, NULL, 0, Aty, CV_GEMM_A_T);
	cvGEMM(invAtA, Aty, 1, NULL, 0, x, 0);
	CvMat* Err = cvCreateMat(row, 1, CV_32FC1);
	cvGEMM(A, x, 1, y, -1, Err, 0);
	float temp, sum = 0.0f;
	for (int i = 0; i < row; i++)
	{
		temp = CV_MAT_ELEM(*Err, float, i, 0);
		sum += temp * temp;
	}
	cvReleaseMat(&AtA);
	cvReleaseMat(&invAtA);
	cvReleaseMat(&Aty);
	cvReleaseMat(&Err);
	return sum;
}